(def font [
    0x00 0x00 0x00 0x00 0x00  0x00 0x00 0x5F 0x00 0x00  0x00 0x07 0x00 0x07 0x00  0x14 0x7F 0x14 0x7F 0x14
    0x24 0x2A 0x7F 0x2A 0x12  0x23 0x13 0x08 0x64 0x62  0x36 0x49 0x56 0x20 0x50  0x00 0x08 0x07 0x03 0x00
    0x00 0x1C 0x22 0x41 0x00  0x00 0x41 0x22 0x1C 0x00  0x2A 0x1C 0x7F 0x1C 0x2A  0x08 0x08 0x3E 0x08 0x08
    0x00 0x80 0x70 0x30 0x00  0x08 0x08 0x08 0x08 0x08  0x00 0x00 0x60 0x60 0x00  0x20 0x10 0x08 0x04 0x02
    0x3E 0x51 0x49 0x45 0x3E  0x00 0x42 0x7F 0x40 0x00  0x72 0x49 0x49 0x49 0x46  0x21 0x41 0x49 0x4D 0x33
    0x18 0x14 0x12 0x7F 0x10  0x27 0x45 0x45 0x45 0x39  0x3C 0x4A 0x49 0x49 0x31  0x41 0x21 0x11 0x09 0x07
    0x36 0x49 0x49 0x49 0x36  0x46 0x49 0x49 0x29 0x1E  0x00 0x00 0x14 0x00 0x00  0x00 0x40 0x34 0x00 0x00
    0x00 0x08 0x14 0x22 0x41  0x14 0x14 0x14 0x14 0x14  0x00 0x41 0x22 0x14 0x08  0x02 0x01 0x59 0x09 0x06
    0x3E 0x41 0x5D 0x59 0x4E  0x7C 0x12 0x11 0x12 0x7C  0x7F 0x49 0x49 0x49 0x36  0x3E 0x41 0x41 0x41 0x22
    0x7F 0x41 0x41 0x41 0x3E  0x7F 0x49 0x49 0x49 0x41  0x7F 0x09 0x09 0x09 0x01  0x3E 0x41 0x41 0x51 0x73
    0x7F 0x08 0x08 0x08 0x7F  0x00 0x41 0x7F 0x41 0x00  0x20 0x40 0x41 0x3F 0x01  0x7F 0x08 0x14 0x22 0x41
    0x7F 0x40 0x40 0x40 0x40  0x7F 0x02 0x1C 0x02 0x7F  0x7F 0x04 0x08 0x10 0x7F  0x3E 0x41 0x41 0x41 0x3E
    0x7F 0x09 0x09 0x09 0x06  0x3E 0x41 0x51 0x21 0x5E  0x7F 0x09 0x19 0x29 0x46  0x26 0x49 0x49 0x49 0x32
    0x03 0x01 0x7F 0x01 0x03  0x3F 0x40 0x40 0x40 0x3F  0x1F 0x20 0x40 0x20 0x1F  0x3F 0x40 0x38 0x40 0x3F
    0x63 0x14 0x08 0x14 0x63  0x03 0x04 0x78 0x04 0x03  0x61 0x59 0x49 0x4D 0x43  0x00 0x7F 0x41 0x41 0x41
    0x02 0x04 0x08 0x10 0x20  0x00 0x41 0x41 0x41 0x7F  0x04 0x02 0x01 0x02 0x04  0x40 0x40 0x40 0x40 0x40
    0x00 0x03 0x07 0x08 0x00  0x20 0x54 0x54 0x78 0x40  0x7F 0x28 0x44 0x44 0x38  0x38 0x44 0x44 0x44 0x28
    0x38 0x44 0x44 0x28 0x7F  0x38 0x54 0x54 0x54 0x18  0x00 0x08 0x7E 0x09 0x02  0x18 0xA4 0xA4 0x9C 0x78
    0x7F 0x08 0x04 0x04 0x78  0x00 0x44 0x7D 0x40 0x00  0x20 0x40 0x40 0x3D 0x00  0x7F 0x10 0x28 0x44 0x00
    0x00 0x41 0x7F 0x40 0x00  0x7C 0x04 0x78 0x04 0x78  0x7C 0x08 0x04 0x04 0x78  0x38 0x44 0x44 0x44 0x38
    0xFC 0x18 0x24 0x24 0x18  0x18 0x24 0x24 0x18 0xFC  0x7C 0x08 0x04 0x04 0x08  0x48 0x54 0x54 0x54 0x24
    0x04 0x04 0x3F 0x44 0x24  0x3C 0x40 0x40 0x20 0x7C  0x1C 0x20 0x40 0x20 0x1C  0x3C 0x40 0x30 0x40 0x3C
    0x44 0x28 0x10 0x28 0x44  0x4C 0x90 0x90 0x90 0x7C  0x44 0x64 0x54 0x4C 0x44  0x00 0x08 0x36 0x41 0x00
    0x00 0x00 0x77 0x00 0x00  0x00 0x41 0x36 0x08 0x00  0x02 0x01 0x02 0x04 0x02
])

(defun putc (x row c)
    (bufcpy pixbuf (+ x 1 (* row 128)) font (* 5 (- c 32)) 5)
)

(defun putstr (x row str)
    (looprange i 0 (str-len str)
        (putc (+ x (* i 6)) row (bufget-u8 str i))
))

(def #SSD-ADDRESS 0x3c)

(def cmds-init '(
        (0xAE)      ; Display off
        (0xD5 0x80) ; Osc freq
        (0xA8 0x3F) ; Mux ratio
        (0xD3 0x00) ; Display offset
        (0x8D 0x14) ; Char reg
        (0x81 0xCF) ; Set contrast
        (0x20 0x00) ; Memory addr mode
        (0x21 0 127) ; Column addr
        (0x22 0 7) ; Page addr
        (0x40) ; Start line
        (0xA1) ; Seg remap op
        (0xC8) ; Com scan dir op
        (0xDA 0x12) ; Com pin conf
        (0xD9 0xF1) ; Precharge
        (0xDB 0x40) ; Vcom deselect
        (0xA4) ; Dis ent disp on
        (0xA6) ; Dis normal
        (0x2E) ; Deactivate scroll
        (0xAF) ; Disaply on
))

(i2c-start 'rate-700k)

(loopforeach c cmds-init
    (i2c-tx-rx #SSD-ADDRESS `(0x00 ,@c))
)

(def pixbuf (bufcreate 1025))
(bufset-u8 pixbuf 0 0x40) ; First byte tells the SSD1306 that this is data
(bufclear pixbuf 0 1)

; Note that the functions below assume int for x, y and row, so a type conversion
; using (to-i x) has to be made if they are called with e.g. floats.

(defun pix-set (x y)
    (bufset-bit pixbuf (+ 8 (* x 8) (mod y 8) (* (/ y 8) 1024)) 1)
)

(defun line (x0 y0 x1 y1) {
        (var dx (abs (- x1 x0)))
        (var sx (if (< x0 x1) 1 -1))
        (var dy (- (abs (- y1 y0))))
        (var sy (if (< y0 y1) 1 -1))
        (var error (+ dx dy))
        
        (loopwhile t {
                (pix-set x0 y0)
                
                (if (and (= x0 x1) (= y0 y1)) (break))
                
                (if (>= (* 2 error) dy) {
                        (if (= x0 x1) (break))
                        (setq error (+ error dy))
                        (setq x0 (+ x0 sx))
                })
                
                (if (<= (* 2 error) dx) {
                        (if (= y0 y1) (break))
                        (setq error (+ error dx))
                        (setq y0 (+ y0 sy))
                })
        })
})

; Nodes and edges of a 3d cube
(def nodes '((-1 -1 -1) (-1 -1 1) (-1 1 -1) (-1 1 1) (1 -1 -1) (1 -1 1) (1 1 -1) (1 1 1)))
(def edges '((0  1) (1 3) (3 2) (2 0) (4 5) (5 7) (7 6) (6 4) (0 4) (1 5) (2 6) (3 7)))

(defun draw-edges () {
        (var scale 17.0)
        (var ofs-x (/ 85 scale))
        (var ofs-y (/ 34 scale))
        
        (loopforeach e edges {
                (var na (ix nodes (ix e 0)))
                (var nb (ix nodes (ix e 1)))
                
                (apply line (map (fn (x) (to-i (* x scale))) (list
                            (+ ofs-x (ix na 0)) (+ ofs-y (ix na 1))
                            (+ ofs-x (ix nb 0)) (+ ofs-y (ix nb 1))
                )))
        })
})

(defun rotate-cube (ax ay) {
        (var sx (sin ax))
        (var cx (cos ax))
        (var sy (sin ay))
        (var cy (cos ay))
        
        (loopforeach n nodes {
                (var x (ix n 0))
                (var y (ix n 1))
                (var z (ix n 2))
                
                (setix n 0 (- (* x cx) (* z sx)))
                (setix n 2 (+ (* z cx) (* x sx)))
                (setvar 'z (ix n 2))
                (setix n 1 (- (* y cy) (* z sy)))
                (setix n 2 (+ (* z cy) (* y sy)))
        })
})

(def fps 0)

(loopwhile t {
        (var t-start (systime))
        (draw-edges)
        (rotate-cube 0.1 0.05)
        (putstr 0 7 (str-from-n fps "FPS %.1f "))
        (i2c-tx-rx #SSD-ADDRESS pixbuf)
        (bufclear pixbuf 0 1)
        (setq fps (/ 1 (secs-since t-start)))
        (sleep 0.01)
})
